#include "cartpole_lcm/cartpole_lcm.h"

CartpoleLcm::CartpoleLcm(/* args */)
{
    pthread_mutex_init(&recvMutex_, NULL);
    pthread_mutex_init(&sendMutex_, NULL);
    lcm_.subscribe("LOWSTATE", &CartpoleLcm::HandleLowState, this);
}

CartpoleLcm::~CartpoleLcm()
{
    pthread_mutex_destroy(&recvMutex_);
    pthread_mutex_destroy(&sendMutex_);
}

void CartpoleLcm::HandleLowState(const lcm::ReceiveBuffer* rbuf, const std::string& chan, const low_state_cartpole_t* msg)
{
    pthread_mutex_lock(&recvMutex_);
    if (msg->timestamp > last_timestamp) {
        recvState_ = *msg;
        last_timestamp = msg->timestamp;
    }
    pthread_mutex_unlock(&recvMutex_);
}

void CartpoleLcm::GetRecv(low_state_cartpole_t& state)
{
    pthread_mutex_lock(&recvMutex_);
    state = recvState_;
    pthread_mutex_unlock(&recvMutex_);
}

void CartpoleLcm::SetSend(low_cmd_cartpole_t& cmd)
{
    pthread_mutex_lock(&sendMutex_);
    sendCommand_ = cmd;
    pthread_mutex_unlock(&sendMutex_);
}

void CartpoleLcm::Send()
{
    pthread_mutex_lock(&sendMutex_);
    lcm_.publish("LOWCMD", &sendCommand_);
    pthread_mutex_unlock(&sendMutex_);
}
